#ifndef HB_ASSIST_LAND_H
#define HB_ASSIST_LAND_H


#include <iostream>
#include <memory>
#include "dji_flight_handler.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "pid.h"



enum cameraZoomControlSizeState{
	cameraZoomHeightLow = 2,
	cameraZoomHeightMiddle = 3,
	cameraZoomHeightHigh = 4,
	cameraZoomHeightHigher = 5,
	cameraZoomHeightHighest = 6
};

enum siftMarkerType{
	siftMarkerNone = 0,
	siftLandMarker = 1,
	siftAlternateLandMarker = 2,
	camerScanModeSearch = 3
};


class djiFlightHandler;

class hbAssistLand
{
public:
	hbAssistLand();
	~hbAssistLand();


	void init(djiFlightHandler *handler);

	void sleepMs(uint32_t timeMs);

	bool getAssistLandModeRunFlag();

	void setAssistLandModeRunFlag(bool flag);

	bool getAssistLandRunFunctionFlag();

	void setAssistLandRunFunctionFlag(bool flag);

	bool getSiftRunFunctionFlag();

	void setSiftRunFunctionFlag(bool flag);

	void setFlightLiveViewFpv(cv::Mat &image);

	void getFlightLiveViewFpv(cv::Mat &image, bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle);

	void setFlightLiveViewMain(cv::Mat &image);

	void getFlightLiveViewMain(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle);

	int getSumOfNoData();

	void setFlightLiveViewVice(cv::Mat &image);

	void getFlightLiveViewVice(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle);

	void setFlightLiveViewTop(cv::Mat &image);

	void getFlightLiveViewTop(cv::Mat &image,  bool &updateFlag, double &flightYaw, T_DjiFcSubscriptionGimbalAngles &gimbalAngle);

	bool startFpvCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	void stopFpvCameraStream(E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	bool startMainCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	void stopMainCameraStream(E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	bool startViceCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	void stopViceCameraStream(E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	bool startTopCameraStream(CameraImageCallback callback, E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	void stopTopCameraStream(E_DjiLiveViewCameraSource source = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT);

	bool getDownStereoCameraParameters(T_DjiPerceptionCameraParameters &cameraParameters);

	bool subscribeDownPerceptionImage();

	bool unsubscribeDownPerceptionImage();

	bool getFrontStereoCameraParameters(T_DjiPerceptionCameraParameters &cameraParameters);

	bool subscribeFrontPerceptionImage();

	bool unsubscribeFrontPerceptionImage();

	E_DjiLiveViewCameraSource getCurrentCameraUse();

	void setCurrentCameraUse(E_DjiLiveViewCameraSource type);
	
	void writeLogFile(const char * format, ...);

	void getGimbalAngles(T_DjiFcSubscriptionGimbalAngles &pry, T_DjiDataTimestamp &timestamp);

	void getGimbalAngles(T_DjiFcSubscriptionGimbalAngles &pry);

	double getGimbalAnglesYaw();

	void computeMoveBearing(cv::Point imageCenter, vector<cv::Point> rect, int inOrEx, double &positionErr, double &rotationAngle, enum siftMarkerType type);

	void computeCorrectBearing(const vector<float> data, double kMeasRadius, double &returnAngle);

	double getFlightCurrentBearing();

	double getFlightCurrentHeigh();

	double getCurrentHeightFusion();

	void setEkfInitSuccessFlag(bool ekfInitFlag);

	bool getEkfInitSuccessFlag();

	bool getNeedAlignCameraYaw();

	void setNeedAlignCameraYaw(bool isNeed);

	void setGoToAlternateLandPoint(bool status);

	bool getStartCameraScanModeFlag();

	void setStartCameraScanModeFlag(bool scanFlag);

	void setSiftHanderStatus(enum siftMarkerType &status);

	enum siftMarkerType getSiftHanderStatus();

	bool obtainJoyStickCtrlAuthority();

	void releaseJoyStickCtrlAuthority();

	bool sendVelocityControlToCopter(T_DjiVector3f velocity,uint32_t timeoutMs,double yawRate = 0);

	bool controlCopterTurnOffMotors();

	bool sendAutoForceLandCommandToCopter();

	bool sendLandPointToCopter(T_DjiFlightControllerHomeLocation homeLocation);

	bool sendStartGoHomeToCopter();

	bool getHaveFlightControlAuthorityFlag();

	double wrap360(const double angle);

	bool gimbalManagerSetModeAndReset(E_DjiGimbalResetMode resetMode = DJI_GIMBAL_RESET_MODE_YAW);

	bool gimbalManagerSetModeAndRestAndRotate();

	bool gimbalManagersetModeAndRotate();

	bool gimbalManagerRotate();

	bool gimbalManagerRotate(double rotateAngle);

	bool cameraScanModeRotateAbsoluteAngle(double rotateRelativeAngle);

	bool cameraScanModeRotateSpeed(double rotateRelativeAngle);

	bool setH20CameraZoom(float multiplier);

	void controlH20CamerZoom(enum cameraZoomControlSizeState &cameraZoomState);

	bool isDoubleGimbal();

	double getFlightInHomeBearing();

	bool sendCancelLandCommandtoCopter();

	bool sendAutoLandCommandToCopter();

	T_DjiFcSubscriptionFlightStatus getFlightCurrentStatus();

	double getDifferenceGimbalYawAndFlightYaw();

	T_DjiFcSubscriptionGimbalControlMode getCurrentGimbalMode();

	T_DjiFcSubscriptionRtkYawInfo getRtkYawInfo();

	void getFlightVelocity(T_DjiFcSubscriptionVelocity &velocity, T_DjiDataTimestamp &timestamp);

	void getAvoidData(T_DjiFcSubscriptionAvoidData &avoidData, T_DjiDataTimestamp &timestamp);

	bool getHomoBearingLockFlag();

	double getComputeLandBearing();

	uint8_t getBatteryInfoPercentage();

	void setHomoBearingLockFlag(double computeLandBearing);

	bool setFlightControlModeByVelocity();

	bool velocityControlToCopter(T_DjiVector3f velocity, double yawRate);

	void resetFlag();

	void resetTranslationSum();

	void setTranslationSum(Vector3f translationAdd);

	void getTranslationSum(Vector3f &translation);

	void setPositionError(Vector3f positionErr);

	void getPositionError(Vector3f &positionErr, bool &updateFlag);

	bool getLandPoint(T_DjiFlightControllerHomeLocation &landPoint);

	bool getCanLandFlag();

	int getNoHeartCount();

	bool goToAlternateLandingPoint();

	void sendCancelLandToMsdk();

	void controlVelocityXY(PID &txPid, PID &tyPid, Vector3f error, T_DjiVector3f &velocity);

	float controlVelocityZ();

	bool startControlFlight();

	bool detecteObstaclesInTheForwardDirection(Vector3f targetPosition);

	double controlYaw(PID &yawPid, double &angleError);

	bool getHaveFixedRtkControlFlag();

	void setHaveFixedRtkControlFlag(bool flag);

	T_DjiFcSubscriptionRtkPositionInfo getRtkPositionInfo();

	bool getIsFixBaseRTKStaus();

	bool whetherStartControlFlight(Vector3f error);

	void assistLand();

	void run();

private:
	djiFlightHandler *m_djiFlightHandler;

	bool m_assistLandModeRunFlag;

	T_DjiFcSubscriptionDisplaymode m_flightCurrentMode;
	T_DjiFcSubscriptionFlightStatus m_flightCurrentStatus;

	bool m_assistLandRunFunctionFlag;
	bool m_siftRunFunctionFlag;
	E_DjiLiveViewCameraSource m_currentCameraUse;

	//obtain fpv stream
	cv::Mat m_flightLiveViewFpv;
	bool m_flightLiveViewFpvUpdateFlag;
	double m_fpvImageFlightYaw;
	T_DjiFcSubscriptionGimbalAngles m_fpvImageGimbalAngle;
	std::mutex m_flightLiveViewFpvLock;

	//obtain main stream
	cv::Mat m_flightLiveViewMain;
	bool m_flightLiveViewMainUpdateFlag;
	double m_mainImageFlightYaw;
	T_DjiFcSubscriptionGimbalAngles m_mainImageGimbalAngle;
	std::mutex m_flightLiveViewMainLock;
	int m_sumOfNoData;

	//obtain Vice stream
	cv::Mat m_flightLiveViewVice;
	bool m_flightLiveViewViceUpdateFlag;
	double m_viceImageFlightYaw;
	T_DjiFcSubscriptionGimbalAngles m_viceImageGimbalAngle;
	std::mutex m_flightLiveViewViceLock;

	//obtain top stream
	cv::Mat m_flightLiveViewTop;
	bool m_flightLiveViewTopUpdateFlag;
	double m_topImageFlightYaw;
	T_DjiFcSubscriptionGimbalAngles m_topImageGimbalAngle;
	std::mutex m_flightLiveViewTopLock;

	//record the flight status of in home
	double m_flightInHomeBearing;
	double m_gimbalInHomeBearing;
	double m_differenceGimbalYawAndFlightYaw;
	T_DjiFcSubscriptionPositionFused m_flightInHomePosition;

	bool m_haveFlightControlAuthorityFlag;
	T_DjiFcSubscriptionGimbalControlMode m_currentGimbalMode;

	T_DjiFcSubscriptionAltitudeBarometer m_groundAltitudeBarometer;

	pthread_t m_landRunTid;
	pthread_t m_siftRunTid;

	bool m_homoBearingLockFlag;
	double m_computeLandBearing;

	Vector3f m_translationSum;
	std::mutex m_translationSumLock;

	Vector3f m_positionError;
	bool m_positionErrorUpdateFlag;
	std::mutex m_positionErrorLock;

	//handler the camera scan mode
	bool m_ekfInitSuccess;
	bool m_startCameraScanMode;
	enum siftMarkerType m_siftHanderStatus;
	bool m_needAlignCameraYaw;
	bool m_goToAlternateLandPoint;

	//FIXED RTK
	bool m_haveFixedRtkControlFlag;

	//callback user data
	char m_fpvName[10] = "FPV_CAM";
	char m_mainName[10] = "MAIN_CAM";
	char m_viceName[10] = "VICE_CAM";
	char m_topName[10] = "TOP_CAM";
};

extern hbAssistLand assistLand;
#endif  //HB_ASSIST_LAND_H
